

#include <string.h>

#include "../../1.Application/Include/APP_WorkingNormalState.h"

#include "../../2.Component/Include/Cmp_Display_Service.h"
#include "../../2.Component/Include/Cmp_SetError.h"
#include "../../2.Component/Include/Cmp_LQC_Process.h"

#include "../../3.Library/Include/general.h"

#include "../../4.Driver/Include/WROS_Register.h"
#include "../../4.Driver/Include/WRRTOS.h"


void DDI_WROS_Init(T_DispInfo*);
void DDI_Task_Start(T_DispInfo*);


#define PRIVILEGED_DATA _Pragma("location=\"WROS_section\"")

static __no_init uint32_t ucHeap[HEAP_SIZE] 				@ 0x20005000;

PRIVILEGED_DATA static volatile UINT32 uSchedulerSuspended					= CLEAR;
PRIVILEGED_DATA static volatile UINT32 uCurrentNumberOfTasks				= CLEAR;
PRIVILEGED_DATA static volatile UINT32 xYieldPending 						= CLEAR;
PRIVILEGED_DATA static volatile UINT32 uxPendedTicks 						= CLEAR;
PRIVILEGED_DATA static volatile UINT32 xSchedulerRunning 					= CLEAR;
PRIVILEGED_DATA static volatile UINT32 uxTopReadyPriority 					= CLEAR;
PRIVILEGED_DATA static volatile UINT32 xNextTaskUnblockTime					= CLEAR;
PRIVILEGED_DATA static volatile UINT32 uxSchedulerSuspended					= CLEAR;
PRIVILEGED_DATA static volatile UINT32 xTickCount 							= CLEAR;
PRIVILEGED_DATA static volatile UINT32 xNumOfOverflows 						= CLEAR;

PRIVILEGED_DATA static UINT32 uxTaskNumber 									= CLEAR;


PRIVILEGED_DATA static List_t pxReadyTasksLists[MAX_PRIORITIES]				= {0};
PRIVILEGED_DATA static List_t xDelayedTaskList1								= {0};	
PRIVILEGED_DATA static List_t xDelayedTaskList2								= {0};	
PRIVILEGED_DATA static List_t xActiveTimerList1 							= {0};
PRIVILEGED_DATA static List_t xActiveTimerList2 							= {0};
PRIVILEGED_DATA static List_t xPendingReadyList								= {0};		
PRIVILEGED_DATA static List_t xTasksWaitingTermination						= {0};
PRIVILEGED_DATA static List_t xSuspendedTaskList							= {0};
PRIVILEGED_DATA static List_t * volatile pxDelayedTaskList					= NULL;	
PRIVILEGED_DATA static List_t * volatile pxOverflowDelayedTaskList			= NULL;	
PRIVILEGED_DATA static List_t * pxCurrentTimerList 							= NULL;
PRIVILEGED_DATA static List_t * pxOverflowTimerList 						= NULL;


PRIVILEGED_DATA TCB_t * volatile pxCurrentTCB								= NULL;


PRIVILEGED_DATA static void* xTimerQueue									= NULL;





static SIZE_T MinimumEverFreeBytesRemaining;
static SIZE_T FreeBytesRemaining;

static UINT32 uCriticalNesting = 0xAAAAAAAA;
extern void vTaskSwitchContext(void);



//	Check it is ture
#define listGET_OWNER_OF_HEAD_ENTRY( pxList )		( (&( ( pxList )->xListEnd ))->pxNext->pvOwner )
#define portSETUP_TCB( pxTCB )						( void ) pxTCB




#define CodePacketStart
#define CodePacketEnd

#define AddTaskToReadyListStart
#define AddTaskToReadyListEnd

/*
void WROS_TaskIDLE(T_DispInfo* psDisplayInfo)	
{
	while(1){}
};
*/
/* Scheduler utilities. */
#define portYIELD()																\
{																				\
	/* Set a PendSV to request a context switch. */								\
	( * ((volatile UINT32 * ) 0xe000ED04)) = ( 1UL << 28UL );					\
																				\
	/* Barriers are normally not required but do ensure the code is completely	\
	within the specified behaviour for the architecture. */						\
	__DSB( 15 );																\
	__ISB( 15 );																\
}



static inline void vPortRaiseBASEPRI(void)
{

	// 直接在汇编中写入优先级数值（假设为3）
	// ulNewBASEPRI = (3 << 4)
	__asm("mov r0, #0x30");
	__asm("msr basepri, r0");
	__asm("dsb");
	__asm("isb");

}

static inline void vPortSetBASEPRI(void)
{
    __asm("mov r0, #0");
    __asm("msr basepri, r0");
}


static void TaskExitError(void)
{
	AlignCheck(uCriticalNesting == ~0UL);
	vPortRaiseBASEPRI();
	for( ;; );
}

static void ENTER_CRITICAL(void)	
{

	vPortRaiseBASEPRI();              // DISENABLE_INTERRUPTS
	uCriticalNesting++;

	if( uCriticalNesting == 1 )
	{
		AlignCheck(((*((volatile UINT32 *)0xe000ed04)) & (0xFFUL)) == 0);
	}
	else
	{	;
	}

}

static void EXIT_CRITICAL(void)	
{

	AlignCheck(uCriticalNesting);
	uCriticalNesting--;
	if( uCriticalNesting == 0 )
	{
		vPortSetBASEPRI();  // ENABLE_INTERRUPTS
	}

}

static UINT32 uxListRemove(ListItem_t * const pxItemToRemove)	
{
	List_t * const pxList = ( List_t * ) pxItemToRemove->pvContainer;
	
	// pxItemToRemove->pxNext->pxPrevious = pxItemToRemove->pxPrevious;
	// pxItemToRemove->pxPrevious->pxNext = pxItemToRemove->pxNext;
	
	// 1. 先用中间变量暂存 volatile 指针的值（只访问一次 volatile）
	ListItem_t * const pxNext = pxItemToRemove->pxNext;    // 暂存 next 指针
	ListItem_t * const pxPrev = pxItemToRemove->pxPrevious;  // 暂存 previous 指针

	// 2. 通过中间变量操作，避免同一表达式中多次访问 volatile
	pxNext->pxPrevious = pxPrev;  // 原行146
	pxPrev->pxNext = pxNext;      // 原行147

	if( pxList->pxIndex == pxItemToRemove )
	{
		pxList->pxIndex = pxItemToRemove->pxPrevious;
	}
	else
	{	;
	}

	pxItemToRemove->pvContainer = NULL;
	( pxList->uxNumberOfItems )--;

	return pxList->uxNumberOfItems;
}

UINT32 xTaskIncrementTick( void )
{
	TCB_t * pxTCB;
	UINT32 xItemValue;
	UINT32 xSwitchRequired = CLEAR;

	if( uxSchedulerSuspended == ( UINT32 ) CLEAR )
	{
		const UINT32 xConstTickCount = xTickCount + ( UINT32 ) 1;

		xTickCount = xConstTickCount;

		if( xConstTickCount == ( UINT32 ) 0U ) /*lint !e774 'if' does not always evaluate to false as it is looking for an overflow. */
		{
			// taskSWITCH_DELAYED_LISTS();	
			List_t *pxTemp;																
																						
			/* The delayed tasks list should be empty when the lists are switched. */	
			AlignCheck( pxDelayedTaskList->uxNumberOfItems == CLEAR );					
																						
			pxTemp = pxDelayedTaskList;													
			pxDelayedTaskList = pxOverflowDelayedTaskList;								
			pxOverflowDelayedTaskList = pxTemp;											
			xNumOfOverflows++;	
			
			// prvResetNextTaskUnblockTime();	
			if(pxDelayedTaskList->uxNumberOfItems != CLEAR )
			{
				xNextTaskUnblockTime = 0xffffffffUL;
			}
			else
			{
				( pxTCB ) = ( TCB_t * ) ((&(pxDelayedTaskList->xListEnd ))->pxNext->pvOwner);
				xNextTaskUnblockTime = ( ( pxTCB )->xStateListItem ).xItemValue;
			}
		}
		else
		{	;
		}

		if( xConstTickCount >= xNextTaskUnblockTime )
		{
			for( ;; )
			{
				if( pxDelayedTaskList->uxNumberOfItems != CLEAR )
				{
					xNextTaskUnblockTime = 0xffffffffUL; /*lint !e961 MISRA exception as the casts are only redundant for some ports. */
					break;
				}
				else
				{
					pxTCB = ( TCB_t * ) ( (&( ( pxDelayedTaskList )->xListEnd ))->pxNext->pvOwner );
					xItemValue = ( &( pxTCB->xStateListItem ) )->xItemValue;

					if( xConstTickCount < xItemValue )
					{
						xNextTaskUnblockTime = xItemValue;
						break;
					}
					else
					{	;
					}

					( void ) uxListRemove( &( pxTCB->xStateListItem ) );

					if( ( &( pxTCB->xEventListItem ) )->pvContainer != NULL )
					{
						( void ) uxListRemove( &( pxTCB->xEventListItem ) );
					}
					else
					{	;
					}

					// prvAddTaskToReadyList( pxTCB );					
					{AddTaskToReadyListStart
						List_t * pxList 			= &( pxReadyTasksLists[ pxTCB->uxPriority ]);
						ListItem_t * pxNewListItem	= &( pxTCB->xStateListItem );
						
						ListItem_t * const pxIndex	= pxList->pxIndex;
						
						//listTEST_LIST_INTEGRITY( pxList );
						//listTEST_LIST_ITEM_INTEGRITY( pxNewListItem );
						
						pxNewListItem->pxNext = pxIndex;
						pxNewListItem->pxPrevious = pxIndex->pxPrevious;
						
						/* Only used during decision coverage testing. */
						//mtCOVERAGE_TEST_DELAY();
						
						pxIndex->pxPrevious->pxNext = pxNewListItem;
						pxIndex->pxPrevious = pxNewListItem;
						
						/* Remember which list the item is in. */
						pxNewListItem->pvContainer = ( void * ) pxList;
						
						( pxList->uxNumberOfItems )++;
					}AddTaskToReadyListEnd

					#if (  configUSE_PREEMPTION == 1 )
					{
						if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority )
						{
							xSwitchRequired = pdTRUE;
						}
						else
						{	;
						}
					}
					#endif /* configUSE_PREEMPTION */
				}
			}
		}

		#if ( ( configUSE_PREEMPTION == 1 ) && ( configUSE_TIME_SLICING == 1 ) )
		{
			if( listCURRENT_LIST_LENGTH( &( pxReadyTasksLists[ pxCurrentTCB->uxPriority ] ) ) > ( UINT32 ) 1 )
			{
				xSwitchRequired = pdTRUE;
			}
			else
			{	;
			}
		}
		#endif /* ( ( configUSE_PREEMPTION == 1 ) && ( configUSE_TIME_SLICING == 1 ) ) */

		#if ( configUSE_TICK_HOOK == 1 )
		{
			if( uxPendedTicks == ( UINT32 ) 0U )
			{
				vApplicationTickHook();
			}
			else
			{	;
			}
		}
		#endif /* configUSE_TICK_HOOK */
	}
	else
	{
		++uxPendedTicks;
	}

	#if ( configUSE_PREEMPTION == 1 )
	{
		if( xYieldPending != CLEAR )
		{
			xSwitchRequired = SET;
		}
		else
		{	;
		}
	}
	#endif /* configUSE_PREEMPTION */

	return xSwitchRequired;
}

static void TaskSuspendAll(void)	
{	++uSchedulerSuspended;
}

static UINT32 TaskResumeAll( void )
{
	TCB_t *pxTCB = NULL;
	UINT32 xAlreadyYielded = RETURN_NG;

	CoupleCheck(uSchedulerSuspended);

	ENTER_CRITICAL();
	{
		--uSchedulerSuspended;

		if(uSchedulerSuspended == (UINT32) CLEAR)
		{
			if(uCurrentNumberOfTasks > (UINT32) 0U)
			{
				while( xPendingReadyList.uxNumberOfItems != CLEAR )
				{
					pxTCB = ( TCB_t * ) listGET_OWNER_OF_HEAD_ENTRY( ( &xPendingReadyList ) );
					( void ) uxListRemove( &( pxTCB->xEventListItem ) );
					( void ) uxListRemove( &( pxTCB->xStateListItem ) );

					
					// prvAddTaskToReadyList( pxTCB );
					{AddTaskToReadyListStart
						List_t * pxList 			= &( pxReadyTasksLists[ pxTCB->uxPriority ]);
						ListItem_t * pxNewListItem	= &( pxTCB->xStateListItem );
						
						ListItem_t * const pxIndex	= pxList->pxIndex;
						
						//listTEST_LIST_INTEGRITY( pxList );
						//listTEST_LIST_ITEM_INTEGRITY( pxNewListItem );
						
						pxNewListItem->pxNext = pxIndex;
						pxNewListItem->pxPrevious = pxIndex->pxPrevious;
						
						/* Only used during decision coverage testing. */
						//mtCOVERAGE_TEST_DELAY();
						
						pxIndex->pxPrevious->pxNext = pxNewListItem;
						pxIndex->pxPrevious = pxNewListItem;
						
						/* Remember which list the item is in. */
						pxNewListItem->pvContainer = ( void * ) pxList;
						
						( pxList->uxNumberOfItems )++;
					}AddTaskToReadyListEnd
					

					if( pxTCB->uxPriority >= pxCurrentTCB->uxPriority )
					{
						xYieldPending = RETURN_OK;
					}
					else
					{	;
					}
				}// xPendingReadyList.uxNumberOfItems != CLEAR

				if( pxTCB != NULL )
				{
					// prvResetNextTaskUnblockTime();
					TCB_t *pxTCB;

					if(pxDelayedTaskList->uxNumberOfItems != CLEAR )
					{
						xNextTaskUnblockTime = 0xffffffffUL;
					}
					else
					{
						( pxTCB ) = ( TCB_t * ) ((&(pxDelayedTaskList->xListEnd ))->pxNext->pvOwner);
						xNextTaskUnblockTime = ( ( pxTCB )->xStateListItem ).xItemValue;
					}
				}// pxTCB != NULL
				else
				{	;
				}
				
				CodePacketStart
				{
					UINT32 uxPendedCounts = uxPendedTicks;

					if( uxPendedCounts > (UINT32) 0U )
					{
						do
						{
							if( xTaskIncrementTick() != RETURN_NG )
							{
								xYieldPending = RETURN_OK;
							}
							else
							{	;
							}
							--uxPendedCounts;
						} while( uxPendedCounts > (UINT32) 0U );

						uxPendedTicks = 0;
					}
					else
					{	;
					}
				}
				CodePacketEnd

				if( xYieldPending != RETURN_NG )
				{
					xAlreadyYielded = RETURN_OK;
					portYIELD();
				}
				else
				{	;
				}
			}// uCurrentNumberOfTasks > (UINT32) 0U
			else
			{	;
			}
		}// uSchedulerSuspended == (UINT32) CLEAR 
		else 
		{	;
		}
	}
	EXIT_CRITICAL();

	return xAlreadyYielded;
}


#define TaskBlockManager

void vListInitialise( List_t * const pxList )
{
	/* The list structure contains a list item which is used to mark the
	end of the list.  To initialise the list the list end is inserted
	as the only list entry. */
	pxList->pxIndex = ( ListItem_t * ) &( pxList->xListEnd );			/*lint !e826 !e740 The mini list structure is used as the list end to save RAM.  This is checked and valid. */

	/* The list end value is the highest possible value in the list to
	ensure it remains at the end of the list. */
	pxList->xListEnd.xItemValue = 0xffffffffUL;

	/* The list end next and previous pointers point to itself so we know
	when the list is empty. */
	pxList->xListEnd.pxNext = ( ListItem_t * ) &( pxList->xListEnd );	/*lint !e826 !e740 The mini list structure is used as the list end to save RAM.  This is checked and valid. */
	pxList->xListEnd.pxPrevious = ( ListItem_t * ) &( pxList->xListEnd );/*lint !e826 !e740 The mini list structure is used as the list end to save RAM.  This is checked and valid. */

	pxList->uxNumberOfItems = ( UINT32 ) 0U;

	/* Write known values into the list if
	configUSE_LIST_DATA_INTEGRITY_CHECK_BYTES is set to 1. */
	// listSET_LIST_INTEGRITY_CHECK_1_VALUE( pxList );
	// listSET_LIST_INTEGRITY_CHECK_2_VALUE( pxList );
}

static UINT32 * pInitialiseStack(UINT32 *pxTopOfStack, void_DispInfo_func pxCode, void * pvParameters)
{
	// Simulate the stack frame as it would be created by a context switch interrupt.
	// Offset added to account for the way the MCU uses the stack on entry/exit of interrupts.
	// xPSR
	pxTopOfStack--; 												
	*pxTopOfStack = (0x01000000);
	// PC
	pxTopOfStack--;
	*pxTopOfStack = ((UINT32) pxCode) & ((UINT32) 0xfffffffeUL);	
	// LR
	pxTopOfStack--;
	*pxTopOfStack = (UINT32) TaskExitError;				
	// R12
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	// R3
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	// R2
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	// R1
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	// R0
	pxTopOfStack --;
	*pxTopOfStack = (UINT32) pvParameters;
	// R11
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	// R10
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	// R9
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	// R8
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	// R7
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	// R6
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	// R5
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	// R4
	pxTopOfStack --;			
	*pxTopOfStack = (0x00000000);
	
	return pxTopOfStack;
}

static void AddNewTaskToReadyList( TCB_t *pxNewTCB )
{
	ENTER_CRITICAL();
	{
		uCurrentNumberOfTasks++;
		if( pxCurrentTCB == NULL )
		{
			pxCurrentTCB = pxNewTCB;

			if( uCurrentNumberOfTasks == ( UINT32 ) 1 )
			{
				// prvInitialiseTaskLists();
				for(UINT32 Priority = (UINT32) 0U; Priority < (UINT32) MAX_PRIORITIES; Priority++ )
				{
					vListInitialise(&(pxReadyTasksLists[Priority]));
				}

				vListInitialise( &xDelayedTaskList1 );
				vListInitialise( &xDelayedTaskList2 );
				vListInitialise( &xPendingReadyList );
				
				vListInitialise( &xTasksWaitingTermination );
				vListInitialise( &xSuspendedTaskList );

				pxDelayedTaskList = &xDelayedTaskList1;
				pxOverflowDelayedTaskList = &xDelayedTaskList2;
			}
			else
			{	;
			}
		}
		else
		{
			/* If the scheduler is not already running, make this task the
			current task if it is the highest priority task to be created
			so far. */
			if( xSchedulerRunning == CLEAR )
			{
				if( pxCurrentTCB->uxPriority <= pxNewTCB->uxPriority )
				{
					pxCurrentTCB = pxNewTCB;
				}
				else
				{	;
				}
			}
			else
			{	;
			}
		}

		uxTaskNumber++;

		/* Add a counter into the TCB for tracing only. */
		pxNewTCB->uxTCBNumber = uxTaskNumber;

		if(pxNewTCB->uxPriority > uxTopReadyPriority)
		{
			uxTopReadyPriority = ( pxNewTCB->uxPriority );
		}
		else
		{	;
		}
		
		//uxTopReadyPriority |= ( 1UL << ( pxNewTCB->uxPriority ) );
		
		// prvAddTaskToReadyList( pxNewTCB );
		{AddTaskToReadyListStart
			List_t* pxList				= &( pxReadyTasksLists[ pxNewTCB->uxPriority ]);
			ListItem_t* pxNewListItem	= &( pxNewTCB->xStateListItem );
			
			ListItem_t* const pxIndex	= pxList->pxIndex;
		
			//listTEST_LIST_INTEGRITY( pxList );
			//listTEST_LIST_ITEM_INTEGRITY( pxNewListItem );
		
			pxNewListItem->pxNext		= pxIndex;
			pxNewListItem->pxPrevious	= pxIndex->pxPrevious;
		
			/* Only used during decision coverage testing. */
			//mtCOVERAGE_TEST_DELAY();
		
			pxIndex->pxPrevious->pxNext	= pxNewListItem;
			pxIndex->pxPrevious			= pxNewListItem;
		
			/* Remember which list the item is in. */
			pxNewListItem->pvContainer	= ( void * ) pxList;
		
			( pxList->uxNumberOfItems )++;
		}AddTaskToReadyListEnd

		portSETUP_TCB( pxNewTCB );
	}
	EXIT_CRITICAL();

	if( xSchedulerRunning != CLEAR )
	{
		/* If the created task is of a higher priority than the current task
		then it should run now. */
		if( pxCurrentTCB->uxPriority < pxNewTCB->uxPriority )
		{
			// taskYIELD_IF_USING_PREEMPTION();
			portYIELD();
		}
		else
		{	;
		}
	}
	else
	{	;
	}
}

static void InitialiseNewTask(struct device* P_DEVICE, T_DispInfo* psDisplayInfo, UINT32* pNewTaskStack, TCB_t* pNewTaskCtlBlk)
{
	//	_REENT_INIT_PTR( ( &( pxNewTCB->xNewLib_reent ) ) );


	//vListInitialiseItem( &( pxNewTCB->xStateListItem ) );
	//vListInitialiseItem( &( pxNewTCB->xEventListItem ) );
	(&(pNewTaskCtlBlk->xStateListItem))->pvContainer	= NULL;
	(&(pNewTaskCtlBlk->xEventListItem))->pvContainer	= NULL;
	
	//listSET_LIST_ITEM_OWNER( &( pxNewTCB->xStateListItem ), pxNewTCB );
	(&(pNewTaskCtlBlk->xStateListItem))->pvOwner		= pNewTaskCtlBlk;

	//listSET_LIST_ITEM_VALUE( &( pxNewTCB->xEventListItem ), ( TickType_t ) configMAX_PRIORITIES - ( TickType_t ) uxPriority ); /*lint !e961 MISRA exception as the casts are only redundant for some ports. */
	//listSET_LIST_ITEM_OWNER( &( pxNewTCB->xEventListItem ), pxNewTCB );
	(&(pNewTaskCtlBlk->xEventListItem))->xItemValue		= ( UINT32 ) MAX_PRIORITIES - (UINT32) P_DEVICE->driver_level; /*lint !e961 MISRA exception as the casts are only redundant for some ports. */
	(&(pNewTaskCtlBlk->xEventListItem))->pvOwner		= pNewTaskCtlBlk;

	// Set "pNewTaskCtlBlk->pxStack" default 0xAA
	pNewTaskCtlBlk->pxStack			= pNewTaskStack;
	memset(pNewTaskCtlBlk->pxStack, ( int ) 0xAA, (SIZE_T)P_DEVICE->driver_StackDepth * sizeof(UINT32));

	// Set "pNewTaskCtlBlk->pxEndOfStack"
	UINT32* pTopOfStack;
	pTopOfStack = pNewTaskCtlBlk->pxStack + (P_DEVICE->driver_StackDepth);
	pTopOfStack = (UINT32 *)(((UINT32) pTopOfStack) & (~((UINT32) 0x0007)));
	AlignCheck((((UINT32)pTopOfStack & (UINT32) 0x0007) == 0UL));
	pNewTaskCtlBlk->pxEndOfStack	= pTopOfStack;

	// Set "pNewTaskCtlBlk->pxTopOfStack"
	pNewTaskCtlBlk->pxTopOfStack	= pInitialiseStack( pTopOfStack, P_DEVICE->driver_api, psDisplayInfo );

	// Set "pNewTaskCtlBlk->uxPriority"
	// Set "pNewTaskCtlBlk->uxBasePriority"
	MAX_Limite(P_DEVICE->driver_level, MAX_PRIORITIES - 1, MAX_PRIORITIES - 1);
	pNewTaskCtlBlk->uxPriority			= P_DEVICE->driver_level;
	pNewTaskCtlBlk->uxBasePriority		= P_DEVICE->driver_level;


	// Set "pNewTaskCtlBlk->pcTaskName" total 16 char
	UINT8 x = 0;
	while(P_DEVICE->name[x] != '\0' && x <= 14)
	{
		pNewTaskCtlBlk->pcTaskName[x]	= P_DEVICE->name[x];
		x++;
	}
	pNewTaskCtlBlk->pcTaskName[15]		= '\0';
	
	// Set "pNewTaskCtlBlk->uxCriticalNesting"
	pNewTaskCtlBlk->uxCriticalNesting	= ( UINT32 ) 0U;

	// Set "pNewTaskCtlBlk->uxMutexesHeld"
	pNewTaskCtlBlk->uxMutexesHeld		= 0UL;
	
	// Set "pNewTaskCtlBlk->ulRunTimeCounter"
	pNewTaskCtlBlk->ulRunTimeCounter	= 0UL;
	
	
	// Set "pNewTaskCtlBlk->ulNotifiedValue"
	// Set "pNewTaskCtlBlk->ucNotifyState"
	pNewTaskCtlBlk->ulNotifiedValue		= 0;
	pNewTaskCtlBlk->ucNotifyState		= taskNOT_WAITING_NOTIFICATION;
	
	// Set "pNewTaskCtlBlk->ucDelayAborted"
	pNewTaskCtlBlk->ucDelayAborted		= CLEAR;
	
	
	if((void*)(&(P_DEVICE->driver_TaskHandle)) != NULL)
	{
		P_DEVICE->driver_TaskHandle = (TCB_t*) pNewTaskCtlBlk;
	}
	else
	{	;
	}
}

static void BlockIntoFreeList(BlockLink_t *pIntoListBlock)
{
	BlockLink_t * pxIterator;
	UINT8 * puc;

	//	Search front free block link
	for(pxIterator = &T_Block_Start; pxIterator->pNextFreeBlock < pIntoListBlock; pxIterator = pxIterator->pNextFreeBlock)
	{
	}
	
	puc = (UINT8*) pxIterator;
	if((puc + pxIterator->BlockSize) == (UINT8*) pIntoListBlock)
	{
		pxIterator->BlockSize	+= pIntoListBlock->BlockSize;
		pIntoListBlock			= pxIterator;
	}
	else
	{	;
	}
	
	puc = (UINT8*) pIntoListBlock;
	if((puc + pIntoListBlock->BlockSize) == (UINT8*) pxIterator->pNextFreeBlock)
	{
		if(pxIterator->pNextFreeBlock != P_Block_End)
		{
			/* Form one big block from the two blocks. */
			pIntoListBlock->BlockSize += pxIterator->pNextFreeBlock->BlockSize;
			pIntoListBlock->pNextFreeBlock = pxIterator->pNextFreeBlock->pNextFreeBlock;
		}
		else
		{
			pIntoListBlock->pNextFreeBlock = P_Block_End;
		}
	}
	else
	{
		pIntoListBlock->pNextFreeBlock = pxIterator->pNextFreeBlock;
	}
	
	if(pxIterator != pIntoListBlock)
	{
		pxIterator->pNextFreeBlock = pIntoListBlock;
	}
	else
	{	;
	}
}

static void * AllocMalloc(SIZE_T RequestSize)
{
	BlockLink_t *p_TotalSection, *p_CurrentBlock, *p_FrontOneBlock, *p_NewBlockLink;
	void *pReturn = NULL;
	
	TaskSuspendAll();
	{
		//	HeapInit
		if( P_Block_End == NULL )
		{
			memset(ucHeap, 0, sizeof(ucHeap));
			
			T_Block_Start.pNextFreeBlock	= (void*)ucHeap;
			T_Block_Start.BlockSize			= (SIZE_T)0;

			P_Block_End 					= (void*)((ucHeap + HEAP_SIZE) - BlockStructStandard);
			P_Block_End->pNextFreeBlock		= (void*)NULL;
			P_Block_End->BlockSize			= (SIZE_T)0;

			p_TotalSection					= (void*)ucHeap;
			p_TotalSection->BlockSize		= (SIZE_T)P_Block_End - (SIZE_T)ucHeap;
			p_TotalSection->pNextFreeBlock	= P_Block_End;

			MinimumEverFreeBytesRemaining	= p_TotalSection->BlockSize;
			FreeBytesRemaining				= p_TotalSection->BlockSize;
		}
		else
		{	;
		}
		
		//	Not over Allocated Size
		if((RequestSize & BlockAllocatedBit) == 0 && (RequestSize <= FreeBytesRemaining))
		{
			RequestSize += sizeof(BlockLink_t);		// Include P_Block_End Size
			AlignSet(RequestSize);
			AlignCheck((RequestSize & 0x0007) == 0);

			p_FrontOneBlock	= &T_Block_Start;
			p_CurrentBlock	= T_Block_Start.pNextFreeBlock;
			while((p_CurrentBlock->BlockSize < RequestSize) && (p_CurrentBlock->pNextFreeBlock != NULL))
			{
				p_FrontOneBlock	= p_CurrentBlock;
				p_CurrentBlock	= p_CurrentBlock->pNextFreeBlock;
			}

			if(p_CurrentBlock != P_Block_End)
			{	
				pReturn = (void*)(((UINT8*) p_FrontOneBlock->pNextFreeBlock) + BlockStructStandard);
				
				// put rear block link with front block link, Current block link release
				p_FrontOneBlock->pNextFreeBlock = p_CurrentBlock->pNextFreeBlock;	
				
				if((p_CurrentBlock->BlockSize - RequestSize) > (BlockStructStandard << 1))
				{
					p_NewBlockLink				= (void*)(((UINT8*) p_CurrentBlock) + RequestSize);
					AlignCheck((((size_t) p_NewBlockLink) & 0x0007) == 0);						// check align 8
					p_NewBlockLink->BlockSize	= p_CurrentBlock->BlockSize - RequestSize;
					p_CurrentBlock->BlockSize	= RequestSize;
					
					BlockIntoFreeList(p_NewBlockLink);
				}
				else
				{	;
				}

				FreeBytesRemaining -= p_CurrentBlock->BlockSize;

				if(FreeBytesRemaining < MinimumEverFreeBytesRemaining)
				{
					MinimumEverFreeBytesRemaining = FreeBytesRemaining;
				}
				else
				{	;
				}
				p_CurrentBlock->BlockSize |= BlockAllocatedBit;
				p_CurrentBlock->pNextFreeBlock = NULL;
			}
			else
			{	;
			}
		}
		else
		{	;
		}
	}
	TaskResumeAll();
	
	AlignCheck((((SIZE_T)pReturn) & 0x0007) == 0);
	return pReturn;
}

static void ReleaseMalloc(void * pRelease)
{
	BlockLink_t *pReleaseBlock = (BlockLink_t*)((UINT8*)pRelease - BlockStructStandard);

	AlignCheck((pReleaseBlock->BlockSize & BlockAllocatedBit) != 0);
	AlignCheck(pReleaseBlock->pNextFreeBlock == NULL);

	if((pReleaseBlock->BlockSize & BlockAllocatedBit) != 0)
	{
		if(pReleaseBlock->pNextFreeBlock == NULL)
		{
			pReleaseBlock->BlockSize &= ~BlockAllocatedBit;

			TaskSuspendAll();
			{
				FreeBytesRemaining += pReleaseBlock->BlockSize;
				BlockIntoFreeList((BlockLink_t *)pReleaseBlock);
			}
			TaskResumeAll();
		}
		else
		{	;
		}
	}
	else
	{	;
	}
}

static UINT32 WROS_TaskCreate(struct device* P_DEVICE, T_DispInfo* psDisplayInfo) 
{
	UINT32*	pNewTask_Stack	= NULL;;
	TCB_t*	pNewTask_CtlBlk	= NULL;

	pNewTask_Stack = (UINT32*)(AllocMalloc((P_DEVICE->driver_StackDepth) * sizeof(UINT32)));
	if(pNewTask_Stack != NULL)
	{
		pNewTask_CtlBlk = (TCB_t*)AllocMalloc(sizeof(TCB_t));
		if(pNewTask_CtlBlk != NULL)
		{			
			InitialiseNewTask(P_DEVICE, psDisplayInfo, pNewTask_Stack, pNewTask_CtlBlk);
			AddNewTaskToReadyList(pNewTask_CtlBlk);
			return RETURN_OK;
		}
		else
		{	
			P_DEVICE->driver_ErrorIndex = A2_Error;
		}
	}
	else
	{
		P_DEVICE->driver_ErrorIndex = A1_Error;
	}
	
	ReleaseMalloc(pNewTask_Stack);

	return RETURN_NG;
}

void DDI_WROS_Init(T_DispInfo* psDisplayInfo)
{
	WROS_TaskCreate(&WROS_DEV_TimerTask,	psDisplayInfo);
	WROS_TaskCreate(&WROS_DEV_PLATFORMCOMM,	psDisplayInfo);
	WROS_TaskCreate(&WROS_DEV_TaskIDLE,		psDisplayInfo);
	WROS_TaskCreate(&WROS_DEV_DISPLAY,		psDisplayInfo);
	WROS_TaskCreate(&WROS_DEV_ERRORCHECK,	psDisplayInfo);
	WROS_TaskCreate(&WROS_DEV_LQCPROCESS,	psDisplayInfo);
}

//  在icf中，声明堆栈8字节对齐
//#pragma required=8
static void xPortPendSVHandler(void)
{
    //PRESERVE8
    __asm("mrs r0, psp");
    __asm("isb");

    __asm("ldr r3, pxCurrentTCB");
    __asm("ldr r2, [r3]");

    __asm("stmdb r0!, {r4-r11}");
    __asm("str r0, [r2]");

    __asm("stmdb sp!, {r3, r14}");
    __asm("mov r0, #5");
    __asm("msr basepri, r0");
    __asm("dsb");
    __asm("isb");
//    __asm("bl vTaskSwitchContext"); // vTaskSwitchContext 还没有定义
    __asm("mov r0, #0");
    __asm("msr basepri, r0");
    __asm("ldmia sp!, {r3, r14}");

    __asm("ldr r1, [r3]");
    __asm("ldr r0, [r1]");
    __asm("ldmia r0!, {r4-r11}");
    __asm("msr psp, r0");
    __asm("isb");
    __asm("bx r14");
    __asm("nop");
}

#define TaskStart

//#pragma required=8
static void prvStartFirstTask( void )
{
	//PRESERVE8
	//__asm("ldr r0, =0xE000ED08");
	//__asm("ldr r0, =0xE000ED08");
/*
    __asm("movw r0, #0xED08"); 
    __asm("movt r0, #0xE000"); 
	__asm("ldr r0, [r0]");
	__asm("ldr r0, [r0]");

	__asm("msr msp, r0");
*/

	__asm("mov r0, #0");
	__asm("msr basepri, r0");
        
        
	__asm("cpsie i");
	__asm("cpsie f");
        
        
	__asm("dsb");
	__asm("isb");
	
/*
	__asm("svc 0");
	__asm("nop");
	__asm("nop");
*/
}

#define configTIMER_QUEUE_LENGTH                 10

static void prvCheckForValidListAndQueue( void )
{
	ENTER_CRITICAL();
	{
		if( xTimerQueue == NULL )
		{
			vListInitialise( &xActiveTimerList1 );
			vListInitialise( &xActiveTimerList2 );
			pxCurrentTimerList	= &xActiveTimerList1;
			pxOverflowTimerList	= &xActiveTimerList2;

			//xTimerQueue = xQueueCreate( ( UINT32 ) configTIMER_QUEUE_LENGTH, sizeof( DaemonTaskMessage_t ) );
		}
		else
		{	;
		}
	}
	EXIT_CRITICAL();
}

static UINT32 xTimerCreateTimerTask(T_DispInfo* psDisplayInfo)
{
	UINT32 xReturn = SET;	//CLEAR;

	prvCheckForValidListAndQueue();

	if( xTimerQueue != NULL )
	{
		//xReturn = WROS_TaskCreate(WROS_TimerTask, psDisplayInfo);
	}
	else
	{	;
	}

	CoupleCheck(xReturn);
	return xReturn;
}


#define portNVIC_SYSTICK_CTRL_REG			( * ( ( volatile uint32_t * ) 0xe000e010 ) )
#define portNVIC_SYSTICK_LOAD_REG			( * ( ( volatile uint32_t * ) 0xe000e014 ) )
#define portNVIC_SYSTICK_CURRENT_VALUE_REG	( * ( ( volatile uint32_t * ) 0xe000e018 ) )
#define portNVIC_SYSPRI2_REG				( * ( ( volatile uint32_t * ) 0xe000ed20 ) )

#define configTICK_RATE_HZ                       ((TickType_t)1000)


//uint32_t SystemCoreClock = 16000000;
#define configCPU_CLOCK_HZ                       ( SystemCoreClock )
#define configSYSTICK_CLOCK_HZ configCPU_CLOCK_HZ
#define portNVIC_SYSTICK_CLK_BIT	( 0 )
#define portNVIC_SYSTICK_INT_BIT			( 1UL << 1UL )
#define portNVIC_SYSTICK_ENABLE_BIT			( 1UL << 0UL )
static void vPortSetupTimerInterrupt(void)
{
/*
	WROS_CORE_M4->NVIC_register.ISER	= 0UL;
	WROS_CORE_M4->NVIC_register.SHP		= 0UL;
	portNVIC_SYSTICK_CTRL_REG = 0UL;
	portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;


	
	WROS_CORE_M4->NVIC_register.CCR		= ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL;
	WROS_CORE_M4->NVIC_register.ISER	= ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT );
	portNVIC_SYSTICK_LOAD_REG = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL;
	portNVIC_SYSTICK_CTRL_REG = ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT );
*/
}

#define configPRIO_BITS								4
#define configLIBRARY_LOWEST_INTERRUPT_PRIORITY		15
#define configKERNEL_INTERRUPT_PRIORITY				( configLIBRARY_LOWEST_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) )
#define portNVIC_PENDSV_PRI							( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 16UL )
#define portNVIC_SYSTICK_PRI						( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 24UL )


#define portFIRST_USER_INTERRUPT_NUMBER		( 16 )
#define portNVIC_IP_REGISTERS_OFFSET_16 	( 0xE000E3F0 )


static UINT16 xPortStartScheduler(void)
{
	#if( configASSERT_DEFINED == 1 )
	{
		volatile uint32_t ulOriginalPriority;
		volatile uint8_t * const pucFirstUserPriorityRegister = ( uint8_t * ) ( portNVIC_IP_REGISTERS_OFFSET_16 + portFIRST_USER_INTERRUPT_NUMBER );
		volatile uint8_t ucMaxPriorityValue;

		ulOriginalPriority = *pucFirstUserPriorityRegister;

		*pucFirstUserPriorityRegister = portMAX_8_BIT_VALUE;

		ucMaxPriorityValue = *pucFirstUserPriorityRegister;

		configASSERT( ucMaxPriorityValue == ( configKERNEL_INTERRUPT_PRIORITY & ucMaxPriorityValue ) );

		ucMaxSysCallPriority = configMAX_SYSCALL_INTERRUPT_PRIORITY & ucMaxPriorityValue;

		ulMaxPRIGROUPValue = portMAX_PRIGROUP_BITS;
		while( ( ucMaxPriorityValue & portTOP_BIT_OF_BYTE ) == portTOP_BIT_OF_BYTE )
		{
			ulMaxPRIGROUPValue--;
			ucMaxPriorityValue <<= ( uint8_t ) 0x01;
		}

		#ifdef __NVIC_PRIO_BITS
		{
			configASSERT( ( portMAX_PRIGROUP_BITS - ulMaxPRIGROUPValue ) == __NVIC_PRIO_BITS );
		}
		#endif

		#ifdef configPRIO_BITS
		{
			configASSERT( ( portMAX_PRIGROUP_BITS - ulMaxPRIGROUPValue ) == configPRIO_BITS );
		}
		#endif

		ulMaxPRIGROUPValue <<= portPRIGROUP_SHIFT;
		ulMaxPRIGROUPValue &= portPRIORITY_GROUP_MASK;

		*pucFirstUserPriorityRegister = ulOriginalPriority;
	}
	#endif

	( * ( ( volatile uint32_t * ) 0xe000ed20 ) ) |= portNVIC_PENDSV_PRI;
	( * ( ( volatile uint32_t * ) 0xe000ed20 ) ) |= portNVIC_SYSTICK_PRI;

	vPortSetupTimerInterrupt();

	uCriticalNesting = 0;

	prvStartFirstTask();

	return 0;
}

void DDI_Task_Start(T_DispInfo* psDisplayInfo)
{
	xTimerCreateTimerTask(psDisplayInfo);

	vPortRaiseBASEPRI();

	xNextTaskUnblockTime = 0xffffffffUL;
	xSchedulerRunning = SET;
	xTickCount = ( UINT32 ) 0U;

	xPortStartScheduler();
	
	(void)WROS_DEV_TaskIDLE.driver_TaskHandle;
}


